/*
 * People.cpp
 *
 *  Created on: Dec 1, 2013
 *      Author: thanhkm
 */

#include "People.h"

People::People() {
	id = rand()%30;
	t =0;
	updated=true;
	image_color = Scalar(rand()%256, rand()%256, rand()%256);
	tl = Point2i(0,0);
	br = Point2i(0,0);
	center = Point2i(0,0);
	last_center = center;
	initKalman(center);
}

People::People(int id_, Mat image_, Point2i tl_, Point2i br_){
//	id = id_;
	id = rand()%30;
	t=0;
	updated=true;
	image_color = Scalar(rand()%256, rand()%256, rand()%256);
	image = image_(Rect(tl_, br_));
	tl = tl_;
	br = br_;
	center = Point2i((tl.x+br.x)/2, (tl.y + br.y)/2);
	last_center = center;
	initKalman(center);
	cout<<"---------------"<<endl;
}

People::~People() {
}

void People::initKalman(Point2i center){
	kf = KalmanFilter(4, 2, 0);
	state = Mat_<float>(4, 1); /* (x, y, Vx, Vy) */
	processNoise = Mat (4, 1, CV_32F);
	measurement = Mat_<float> (2,1);
	measurement.setTo(Scalar(0));

	kf.init(4,2,0);
	kf.transitionMatrix = *(Mat_<float>(4, 4) << 1, 0, 1, 0,
												 0, 1, 0, 1,
												 0, 0, 1, 0,
												 0, 0, 0, 1);
//	kf.processNoiseCov = *(Mat_<float>(4, 4) <<  0.2, 0,   0.2, 0,
//												 0,   0.2, 0,   0.2,
//												 0,   0,   0.2, 0,
//												 0,   0,   0,   0.2);
	setIdentity(kf.measurementMatrix);
	setIdentity(kf.processNoiseCov, Scalar::all(1e-5));
	setIdentity(kf.measurementNoiseCov, Scalar::all(1e-1));
	setIdentity(kf.errorCovPost, Scalar::all(0.1));

	kf.statePost = *(cv::Mat_<float>(4, 1) << center.x,center.y,0,0);
	kf.statePre = *(cv::Mat_<float>(4, 1) << center.x,center.y,0,0);
}

Point2i People::correctKalman(Point2i center){
	Mat prediction = kf.predict();
	Point predictPt(prediction.at<float>(0),prediction.at<float>(1));

	posPre.push_back(predictPt);

	measurement(0) = center.x;
	measurement(1) = center.y;

	Point measPt(measurement(0),measurement(1));
	posMer.push_back(measPt);

	// generate measurement
	measurement += kf.measurementMatrix*state;

	Mat estimated = kf.correct(measurement);
	Point statePt(estimated.at<float>(0),estimated.at<float>(1));
	posCorr.push_back(statePt);

	return statePt;
}

void People::update(Mat image_, Point2i tl_, Point2i br_){
	last_center = center;
	//compare rect actual with rect update
	//if rect update is smaller, pre-process before update
	Rect rectImage(tl,br);
	Rect rectUpdate(tl_,br_);

	float dist1 = sqrt(rectImage.height*rectImage.height + rectImage.width * rectImage.width);
	float dist2 = sqrt(rectUpdate.height*rectUpdate.height + rectUpdate.width * rectUpdate.width);

	if (dist1 > 1.5 * dist2){
		cout<<"two small not update-----------------------------"<<endl;

	} else if (dist1 > 1.1 * dist2){
		Point2i pt= Point2i((tl_.x+br_.x)/2, (tl_.y + br_.y)/2);
		pt = dist2/dist1 *(pt-center);
		//just move center of object
		moveCenter(image_,pt);

	} else if (dist1 <= 1.1 * dist2){
		image = image_(Rect(tl_, br_));
		tl = tl_;
		br = br_;
		center = Point2i((tl.x+br.x)/2, (tl.y + br.y)/2);
	//	cout<<"update-----"<<tl<<br<<endl;
	}
	Point pt = correctKalman(center);
	//il faut update le centre
	//..............
	moveCenter(image_, pt - center);
}

bool People::isMatchedPeople(Mat image_, Point2i tl_, Point2i br_){

	Rect rectImage(tl,br);
	Rect rectUpdate(tl_,br_);
	//minimum area rectangle containing rectImage and rectUpdate
	Rect rect=rectImage | rectUpdate;

	if (1.5 * rectImage.area() > rect.area()){
		cout<< "matched rectangle:1: "<<rectImage.area() <<" 2: "<< rect.area()<<endl;
//		return true;
	} else if(2.0 * rectImage.area() > rect.area()){
		cout<< "not matched rectangle:1: "<<rectImage.area() <<" 2: "<< rect.area()<<endl;
	}


	float distance = dist(getMidPoint(tl_,br_),center);
	if (distance > 20){
//		cout<<"people not matched: distance is long--------------"<<endl;
		return false;
	} else if(distance < 12){
//		cout<<"people matched distance--------------"<<endl;
		return true;
	}


	tl_.x = tl_.x - 4;
	tl_.y = tl_.y - 4;
	br_.x = br_.x + 4;
	br_.y = br_.y + 4;
	Rect r = Rect(tl_, br_);
	Rect size = Rect(Point2i(0,0), image_.size());
	r = r & size;

	cv::Mat img = image_(r);
	cout<<"size check"<<tl_<<br_<<endl;

	if (compareFeatures(image, img)>=2){
		cout<<"people matched SUFT---------------"<<endl;
		return true;
	}

	cout<<"people not matched:distance and key-----------"<<endl;
	return false;
}

bool People::isInScence(Mat image_){
	bool result = false;

	t = t+1;
	if (updated){
		t = 0;
		updated = false;
		return true;
	}

	if (t<=170){
		return true;
	}

	if (t>170){
		return false;
	}

//	Mat img = image_(Rect(tl, br));
//	if (compareFeatures(image, img)>=2){
//		return true;
//	}

	return result;
}

int People::compareFeatures(Mat img1,Mat img2){
//	MSER mser;
//	vector<KeyPoint> k;
//	mser.detect(img1,k);
//	cout<<"mser detector nb key points: "<<k.size()<<"-----------"<<endl;

    // detecting keypoints
    SiftFeatureDetector detector(200);
    vector<KeyPoint> keypoints1, keypoints2;
    detector.detect(img1, keypoints1);
    detector.detect(img2, keypoints2);

    // computing descriptors
    SiftDescriptorExtractor extractor;
    Mat descriptors1, descriptors2;
    extractor.compute(img1, keypoints1, descriptors1);
    extractor.compute(img2, keypoints2, descriptors2);

    if(keypoints1.size()>2){
    	cout<<"nb key points 1: "<<keypoints1.size()<<" 2: "<<keypoints2.size()<<endl;
    }
    // if no key points return 0
    if(keypoints1.size() * keypoints2.size() == 0){
    	return 0;
    }
    // matching descriptors
    // Matching descriptor vectors using FLANN matcher
    FlannBasedMatcher matcher;
    std::vector< DMatch > matches;
    matcher.match( descriptors1, descriptors2, matches );

    double max_dist = 0; double min_dist = 100;
    // Quick calculation of max and min distances between keypoints
    for( int i = 0; i < descriptors1.rows; i++ ){
    	double dist = matches[i].distance;
    	if( dist < min_dist ) min_dist = dist;
    	if( dist > max_dist ) max_dist = dist;
    }

//    printf("-- Max dist : %f \n", max_dist );
//    printf("-- Min dist : %f \n", min_dist );

    // Get "good" matches (i.e. whose distance is less than 3*min_dist )
    std::vector< DMatch > good_matches;
    for( int i = 0; i < descriptors1.rows; i++ ){
    	if( matches[i].distance <= 3.5*min_dist ){
    		good_matches.push_back( matches[i]);
    	}
    }
    cout<<good_matches.size() << endl;
	return good_matches.size();
}

float People::compareHistogram(Mat img1,Mat img2){
	float result=0;
	MatND hist1;
	MatND hist2;

	/// Using 30 bins for channel
	int g_bins = 50;
	int histSize[] = { g_bins };
	// hue varies from 0 to 256, saturation from 0 to 180
	float g_ranges[] = { 0, 256 };

	const float* ranges[] = { g_ranges };

	// Use the o-th channels
	int channels[] = { 0};
	/// Calculate the histograms for the images
	calcHist( &img1, 1, channels, Mat(), hist1, 2, histSize, ranges, true, false );
	normalize( hist1, hist1, 0, 1, NORM_MINMAX, -1, Mat() );
	calcHist( &img2, 1, channels, Mat(), hist2, 2, histSize, ranges, true, false );
	normalize( hist2, hist2, 0, 1, NORM_MINMAX, -1, Mat() );
	// get correlation between img1 and img2
	int compare_method = 1;
	result = compareHist( hist1, hist2, compare_method );

	return result;
}

Rect People::getRect(){
	return Rect(tl, br);
}

Point2i People::getLastPos(){
	return last_center;
}

Point2i People::getPos(){
	return center;
}

Scalar People::getColor(){
	return image_color;
}

void People::setUpdated(bool value){
	updated = value;
}

void People::moveCenter(Mat img, Point2i pt){
	Rect r = Rect(tl, br);
	Rect size = Rect(Point2i(0,0), img.size());
	r = r + pt;
	// intersec for not out of video size
	r = r & size;

	image = img(r);
	tl = r.tl();
	br = r.br();
	center = Point2i((tl.x+br.x)/2, (tl.y + br.y)/2);
}

void People::changeCenter(Mat img, Point2i newCenter){
	moveCenter(img, newCenter - center);
}

Point2i People::getPre(){
	return posPre[posPre.size()-1];
}
Point2i People::getLastPre(){
	int i = posPre.size();
	if (i < 1){
		return posPre[i-1];
	}
	return posPre[i-2];
}

Point2i People::getCorr(){
	return posCorr[posCorr.size()-1];
}
Point2i People::getLastCorr(){
	int i = posCorr.size();
	if (i < 1){
		return posCorr[i-1];
	}
	return posCorr[i-2];
}
//Point2i getCorr();
//Point2i getLastPre();



vector<Point2i> People::getvPre(){
	return posPre;
}
vector<Point2i> People::getvMer(){
	return posMer;
}
vector<Point2i> People::getvCor(){
	return posCorr;
}
